/*
 * Copyright (C) 2023 HiHope Open Source Organization .
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 *
 * limitations under the License.
 */

#include <stdio.h>
#include <unistd.h>
#include <string.h>

#include "ohos_init.h"
#include "cmsis_os2.h"

#include "hi_io.h"
#include "iot_gpio.h"

#include "pzcar_i2c_ctrl.h"
#include "pzcar_wheel_ctrl.h"

extern PzCarInfo g_carInfo;

char* eCm_String[] = {
    "Auto",
    "Track",
    "AudCmd",
    "NetCtrl",
    "Max",
};

char* eSt_String[] = {
    "Stop",
    "Forward",
    "TurnLeft",
    "TurnRight",
    "Backward",
    "BackLeft",
    "BackRight",
    "Max",
};

void WheelSpeedJudge(const signed short speed)
{
    if ((speed < g_carInfo.d_speed_min) || (speed > g_carInfo.d_speed_max)) {
        printf("%s: speed[%d] invalid, set current speed to default speed[%d]\n",
            __func__, speed, g_carInfo.d_speed_def);
        g_carInfo.d_speed_curr = g_carInfo.d_speed_def;
    }
    else {
        g_carInfo.d_speed_curr = speed;
        //printf("%s: speed[%d] valid, update to current speed[%d]\n",
        //    __func__, speed, g_carInfo.d_speed_curr);
    }
}


// speed: Hi[MSB]Lo[LSB]: [-3600, 3600]=[0xF1F0, 0x0E10]
// duration: 0-keep moving, xxx--ms
void WheelMovementCtrl(CarState eSt, const signed short speed, const unsigned short duration)
{
    uint8_t data[5] = {0x18, 0x00, 0x00, 0x00, 0x00};
    uint8_t stop[5] = {0x18, 0x00, 0x00, 0x00, 0x00};
    WheelSpeedJudge(speed);
    signed short spd = g_carInfo.d_speed_curr;

    if (eSt == eSt_Stop) {
        I2C0_Write(stop, 5);
        return;
    } if (eSt == eSt_Forward) {
        data[1] = (spd & 0x00FF) >> 0;
        data[2] = (spd & 0xFF00) >> 8;
        data[3] = data[1];
        data[4] = data[2];
    } else if (eSt == eSt_TurnLeft) {
        data[1] = (spd & 0x00FF) >> 0;
        data[2] = (spd & 0xFF00) >> 8;
        data[3] = ((spd/2) & 0x00FF) >> 0;
        data[4] = ((spd/2) & 0xFF00) >> 8;
    } else if (eSt == eSt_TurnRight) {
        data[1] = ((spd/2) & 0x00FF) >> 0;
        data[2] = ((spd/2) & 0xFF00) >> 8;
        data[3] = (spd & 0x00FF) >> 0;
        data[4] = (spd & 0xFF00) >> 8;
    } else if (eSt == eSt_Backward) {
        data[1] = (((-1)*spd) & 0x00FF) >> 0;
        data[2] = (((-1)*spd) & 0xFF00) >> 8;
        data[3] = data[1];
        data[4] = data[2];
    } else if (eSt == eSt_BackLeft) {
        data[1] = (((-1)*spd) & 0x00FF) >> 0;
        data[2] = (((-1)*spd) & 0xFF00) >> 8;
        data[3] = (((-1)*(spd/2)) & 0x00FF) >> 0;
        data[4] = (((-1)*(spd/2)) & 0xFF00) >> 8;
    } else if (eSt == eSt_BackRight) {
        data[1] = (((-1)*(spd/2)) & 0x00FF) >> 0;
        data[2] = (((-1)*(spd/2)) & 0xFF00) >> 8;
        data[3] = (((-1)*spd) & 0x00FF) >> 0;
        data[4] = (((-1)*spd) & 0xFF00) >> 8;
    } else {
        printf("%s: eSt[%d] miss-matched, do nothing...\n", __func__, eSt);
        return;
    }

    I2C0_Write(data, 5);
    if (duration) {
        usleep(duration * 1000);
        I2C0_Write(stop, 5);
    }
}

void WheelMovementCmd(const char* cmd, const signed short speed, const unsigned short duration)
{
    printf("%s: cmd[%s], speed[%d], duration[%d]\n", __func__, cmd, speed, duration);
    
    if (strncmp(cmd, "Stop", sizeof("Stop")) == 0) {
        WheelMovementCtrl(eSt_Stop, speed, duration);
    } if (strncmp(cmd, "Forward", sizeof("Forward")) == 0) {
        WheelMovementCtrl(eSt_Forward, speed, duration);
    } else if (strncmp(cmd, "TurnLeft", sizeof("TurnLeft")) == 0) {
        WheelMovementCtrl(eSt_TurnLeft, speed, duration);
    } else if (strncmp(cmd, "TurnRight", sizeof("TurnRight")) == 0) {
        WheelMovementCtrl(eSt_TurnRight, speed, duration);
    } else if (strncmp(cmd, "Backward", sizeof("Backward")) == 0) {
        WheelMovementCtrl(eSt_Backward, speed, duration);
    } else if (strncmp(cmd, "BackLeft", sizeof("BackLeft")) == 0) {
        WheelMovementCtrl(eSt_BackLeft, speed, duration);
    } else if (strncmp(cmd, "BackRight", sizeof("BackRight")) == 0) {
        WheelMovementCtrl(eSt_BackRight, speed, duration);
    } else {
        printf("%s: cmd[%s] miss-matched, do nothing...\n", __func__, cmd);
        return;
    }
}

void WheelCmd(const char* cmd)
{
    WheelMovementCmd(cmd, g_carInfo.d_speed_def, g_carInfo.d_duration_def);
}

void WheelModeAuto8(void)  // Run 8
{
    signed short speed = 1500;
    unsigned short dura[2] = {2000, 800};
    CarState eStPatten[] = {
        eSt_Forward,
        eSt_TurnLeft,
        eSt_Forward,
        eSt_TurnLeft,
        eSt_Forward,
        eSt_TurnLeft,

        eSt_Forward,
        eSt_TurnRight,
        eSt_Forward,
        eSt_TurnRight,
        eSt_Forward,
        eSt_TurnRight,

        eSt_Stop,
        eSt_Stop,

        eSt_Backward,
        eSt_BackLeft,
        eSt_Backward,
        eSt_BackLeft,
        eSt_Backward,
        eSt_BackLeft,

        eSt_Backward,
        eSt_BackRight,
        eSt_Backward,
        eSt_BackRight,
        eSt_Backward,
        eSt_BackRight,

        eSt_Stop,
        eSt_Stop,
    };

    unsigned char len = sizeof(eStPatten)/sizeof(CarState);
    while (g_carInfo.e_CM == eMode_Auto) {
        for (unsigned char i=0; i<len; i++) {
            if (g_carInfo.e_CM != eMode_Auto) {
                WheelMovementCtrl(eSt_Stop, 0, 0);
                break;
            }
            g_carInfo.e_ST = eStPatten[i];
            g_carInfo.d_speed_curr = speed;
            WheelMovementCtrl(g_carInfo.e_ST, speed, dura[i%2]);
            if (g_carInfo.e_ST == eSt_Stop) {
                usleep(dura[0]*1000);
            }
        }
    }
}

void WheelModeAuto0(void)
{
    #define DATA_LEN   (5)
    #define STEP_CNT   (12)
    unsigned short dura[STEP_CNT] =
    {
        3000, 3750,
        3000, 3750,
        3000, 1000,
        3000, 3750,
        3000, 3750,
        3000, 1000
    };
    signed short speed[STEP_CNT][2] =
    { // Left, Right
        {2400, 2400}, {1200, 2400},
        {2400, 2400}, {2400, 1200},
        {2400, 2400}, {0, 0},
        {-2400, -2400}, {-2400, -1200},
        {-2400, -2400}, {-1200, -2400},
        {-2400, -2400}, {0, 0}
    };
    CarState eStList[STEP_CNT] =
    {
        eSt_Forward, eSt_TurnRight,
        eSt_Forward, eSt_TurnLeft,
        eSt_Forward, eSt_Stop,
        eSt_Backward, eSt_BackLeft,
        eSt_Backward, eSt_BackRight,
        eSt_Backward, eSt_Stop
    };
    uint8_t data[DATA_LEN] = {0x18, 0x00, 0x00, 0x00, 0x00};
    uint8_t stop[DATA_LEN] = {0x18, 0x00, 0x00, 0x00, 0x00};

    while (g_carInfo.e_CM == eMode_Auto) {
        for (unsigned char i=0; i<STEP_CNT; i++) {
            g_carInfo.e_ST = eStList[i];
            data[1] = (speed[i][0] & 0x00FF) >> 0;
            data[2] = (speed[i][0] & 0xFF00) >> 8;
            data[3] = (speed[i][1] & 0x00FF) >> 0;
            data[4] = (speed[i][1] & 0xFF00) >> 8;
            I2C0_Write(data, DATA_LEN);
            usleep(dura[i]*1000);
            if (g_carInfo.e_CM != eMode_Auto) {
                g_carInfo.e_ST = eSt_Stop;
                I2C0_Write(stop, DATA_LEN);
                return;
            }
        }
    }
    g_carInfo.e_ST = eSt_Stop;
    I2C0_Write(stop, DATA_LEN);
}

void WheelModeTrack(void)
{
    #define READ_LEN (3)
    unsigned char data[READ_LEN] = {0x1C, 0x00, 0x00};
    I2C0_Read(data, READ_LEN);
    if (data[1] || data[2]) {
        printf("%s: ITR_L[0x%02X] ITR_R[0x%02X]\n", __func__, data[1], data[2]);
    }
}

void WheelTask(void)
{
    CarMode eCm = eMode_Max;
    CarState eSt = eSt_Stop;

    int apInited = -1;
    signed short speed = 0;
    unsigned short duration = 0;

    while (1) {
        #if 0  //Track mode NOT ready yet, and will cause I2C0 Fail
        if (g_carInfo.e_CM == eMode_Track) {
            eCm = g_carInfo.e_CM;
            WheelModeTrack();
            usleep(200*1000);
            continue;
        }
        #endif

        if ((eCm == g_carInfo.e_CM) &&
            (eSt == g_carInfo.e_ST) &&
            (speed == g_carInfo.d_speed_curr) &&
            (duration == g_carInfo.d_duration)) {
            usleep(500*1000);
            continue;
        }
        eCm = g_carInfo.e_CM;
        eSt = g_carInfo.e_ST;
        speed = g_carInfo.d_speed_curr;
        duration = g_carInfo.d_duration;

        switch (g_carInfo.e_CM) {
        case eMode_Auto:
            WheelModeAuto0();
            break;
        case eMode_Track:
        case eMode_AudCmd:
        case eMode_NetCtrl:  
            WheelMovementCtrl(g_carInfo.e_ST, g_carInfo.d_speed_curr, g_carInfo.d_duration);
            break;
        default:
            break;
        }
        usleep(100*1000);
    }
}

osThreadId_t WheelTaskEntry(void)
{
    //osPriorityAboveNormal[32], osPriorityNormal[24]
    //{.name, .attr_bits, .cb_mem, .cb_size, .stack_mem, .stack_size, .priority, .tz_module, .reserved}
    osThreadAttr_t attr = {"WheelTask", 0, NULL, 0, NULL, 1024*4, 24, 0, 0};
    osThreadId_t taskId = osThreadNew(WheelTask, NULL, &attr);

    if (taskId == NULL) {
        printf("%s: Failed to create %s!\n", __func__, attr.name);
        return NULL;
    }
    return taskId;
}
//SYS_RUN(WheelTasktEntry);